package  projects.ufam.filters;

import java.awt.geom.Point2D;
import java.text.DecimalFormat;
import java.util.Random;

import org.jfree.data.statistics.Statistics;

import sinalgo.tools.Tools;

public class ParticleFilter {
	
	private Particle[] particles;
    //private double[] obsAnt = new double[2];
	private Random rand = Tools.getRandomNumberGenerator(); 
	
	
	public ParticleFilter( int particleNumber ) {
				particles = new Particle[particleNumber];
		for( int i=0; i<particles.length; i++ )
			particles[i] = new Particle( rand.nextGaussian()*100, rand.nextGaussian()*100, 0, 0, 0 );
	}
	
	public int particlesNumber() {
		return particles.length;
	}
	
	public Particle[] getParticles() {
		return this.particles;
	}

	public double[] step( double obs[] ) {
		sampling();
		importance( obs );
		resampling();

		double[] p = new double[4];
		for(int i=0; i<particlesNumber(); i++) {
			p[0] += particles[i].getXPosition() * particles[i].getWeight();
			p[1] += particles[i].getYPosition() * particles[i].getWeight();
			p[2] += particles[i].getXVelocity() * particles[i].getWeight();
			p[3] += particles[i].getYVelocity() * particles[i].getWeight();
		}
		return p;
	}
	
	// Aplica o modelo de dinamica (modifica a matriz particulas) -- Sampling/predict
	private void sampling() {
		for ( int i=0; i<particlesNumber(); i++ ) {
			double x  = ( particles[i].getXPosition() + particles[i].getXVelocity() ) + (rand.nextGaussian());
			double y  = ( particles[i].getYPosition() + particles[i].getYVelocity() ) + (rand.nextGaussian());
			double xv = particles[i].getXVelocity() + (rand.nextGaussian()*0.1); //0.05
			double yv = particles[i].getYVelocity() + (rand.nextGaussian()*0.1); //0.05
			particles[i].setXVelocity(xv); 
			particles[i].setYVelocity(yv);
			particles[i].setXPosition(x);
			particles[i].setYPosition(y);
		}
	}
	
	// Peso (modifica o vetor peso) -- Importance/weighting
	private void importance( double[] obs ) {
		double sumWeights = 0;
		for( int i=0; i<particlesNumber(); i++ ) {
			double x = obs[0];
			double y = obs[1];
			double d = Point2D.distance(x, y, particles[i].getXPosition(), particles[i].getYPosition());
			particles[i].setWeight(1.0/d);
			sumWeights += particles[i].getWeight();
		}
		//Normalização
		for( int i=0; i<particlesNumber(); i++ ) 
			particles[i].setWeight( particles[i].getWeight()/sumWeights );
	}
	
	// Reamonstragem (modifica a matriz particulas)
	private void resampling() {
		int size = this.particlesNumber();
		double[] slice = new double[size];
		Particle[] particlesResampling = new Particle[size];
		Random random = new Random();

		slice[0] = particles[0].getWeight();
		for (int i = 1; i<size; i++) slice[i] = slice[i-1] + particles[i].getWeight();
		
		for( int i=0; i<size; i++ ) {
			double choice = random.nextFloat();
			int j=0;
			while( (j < size-1) && (slice[j] < choice ) ) j++;
			particlesResampling[i] = particles[j].copyParticle();
			particlesResampling[i].setWeight(1.0/size);
		}
		this.particles = particlesResampling;
	}

	public class Particle {
		private double xPosition;
		private double yPosition;
		private double xVelocity;
		private double yVelocity;
		private double weight;
		
		public Particle() {
			this(0, 0, 0, 0, 0);
		}
		
		public Particle( double xPosition, double yPosition, double xVelocity, double yVelocity, double weight ) {
			setXPosition(xPosition);
			setYPosition(yPosition);
			setXVelocity(xVelocity);
			setYVelocity(yVelocity);
			setWeight(weight);
		}
		
		public Particle copyParticle() {
			return new Particle( this.getXPosition(), this.getYPosition(), this.getXVelocity(), this.getYVelocity(), this.getWeight() );
		}
		
		public void setXPosition( double xPosition ) {
			this.xPosition = xPosition;
		}
		
		public double getXPosition() {
			return this.xPosition;
		}
		
		public void setYPosition( double yPosition ) {
			this.yPosition = yPosition;
		}
		
		public double getYPosition() {
			return this.yPosition;
		}
		
		public void setXVelocity( double xVelocity ) {
			this.xVelocity = xVelocity;
		}
		
		public double getXVelocity() {
			return this.xVelocity;
		}
		
		public void setYVelocity( double yVelocity ) {
			this.yVelocity = yVelocity;
		}
		
		public double getYVelocity() {
			return this.yVelocity;
		}
		
		public void setWeight( double weight ) {
			this.weight = weight;
		}
		
		public double getWeight() {
			return this.weight;
		}

        public String toString() {
            DecimalFormat dc = new DecimalFormat("#.##");
            dc.setMinimumFractionDigits(2);
            dc.setMaximumFractionDigits(2);
            return "x:" + dc.format(xPosition) + ", y:" + dc.format(yPosition)
                    + ", vx:" +  dc.format(xVelocity) + ", vy:" + dc.format(yVelocity) + ", w:" + dc.format(weight);
        }
	}
	
	public static void main( String args[] ) {
		ParticleFilter pf = new ParticleFilter(1000);
		Random rand = new Random();
        double mean_ee = 0;
        double mean_em = 0;
        int numberSteps = 1000;
		
		//Point Real
        double x = 0;
        double y = 0;
        double d = 0.1;
        double a = 0;
        double xm=0, ym=0;

        for( int k=0; k<numberSteps; k++) {
        	
        	double[] e = pf.step(new double[] {xm, ym} );
			double xe = e[0];
			double ye = e[1];

            if ( k % 25 == 0 ) {
                //Sortear um angulo que vai de +45 a -45 deste angulo
                double tmp = rand.nextInt(45);
                if ( rand.nextBoolean() )
                    a = (a + tmp)%360;
                else
                    a = (tmp <= a ? a-tmp : a+360-tmp );
            }
            double ca = Math.cos(Math.toRadians(a))*d;
            double co = Math.sin(Math.toRadians(a))*d;
            x += ca;
            y += co;
            
        	//Acrescentando ruido
        	xm = x + rand.nextGaussian();
        	ym = y + rand.nextGaussian();

        	//Calculando o erro da medição e da estimação
            mean_ee += Point2D.distance(x, y, xe, ye);
            mean_em += Point2D.distance(x, y, xm, ym);
			System.out.println( x+","+y + "\t" + xe+","+ye+ "\t"+ xm+","+ym );
        }
        System.out.println("Erro Medio da Medicao = " + (mean_em/numberSteps));
        System.out.println("Erro Medio da Estimacao = " + (mean_ee/numberSteps));
	}
}